///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_TRANSFORMATIONS_H_
#define _BUOLA_MAT_TRANSFORMATIONS_H_

#include <buola/mat.h>
#include <buola/mat/c2dtransform.h>
#include <buola/mat/cquat.h>
#include <buola/mat/c3drotation.h>
#include <buola/mat/c3drottrans.h>
#include <buola/utility/csize.h>
#include <buola/utility/cslope.h>

namespace buola { namespace mat {

inline CMat23d translation_2d(const CPoint_d &pT)
{
    return {1.0,0.0,pT.x,
            0.0,1.0,pT.y};
}

inline CMat23f translation_2f(const CPoint_f &pT)
{
    return {1.0,0.0,pT.x,
            0.0,1.0,pT.y};
}

inline CMat23d scaling_2d(double pS)
{
    return {pS ,0.0,0.0,
            0.0,pS ,0.0};
}

inline CMat23f scaling_2f(float pS)
{
    return {pS ,0.0,0.0,
            0.0,pS ,0.0};
}

inline CMat23d scaling_2d(const CSize_d &pS)
{
    return {pS.x,0.0 ,0.0,
            0.0 ,pS.y,0.0};
}

inline CMat23f scaling_2f(const CSize_f &pS)
{
    return {pS.x,0.0 ,0.0,
            0.0 ,pS.y,0.0};
}

inline CMat23d rotation_2d(double pA)
{
    double lSin,lCos;
    sincos(pA,&lSin,&lCos);

    return {lCos,-lSin,0.0,
            lSin,lCos ,0.0};
}

inline CMat23f rotation_2f(float pA)
{
    float lSin,lCos;
    sincosf(pA,&lSin,&lCos);

    return {lCos,-lSin,0.0,
            lSin,lCos ,0.0};
}

inline double determinant(const CMat23d &pM)
{
    return pM(0,0)*pM(1,1)-pM(0,1)*pM(1,0);
}

inline float determinant(const CMat23f &pM)
{
    return pM(0,0)*pM(1,1)-pM(0,1)*pM(1,0);
}

inline mat::CMat23d inverse(const CMat23d &pM)
{
    double lDet=determinant(pM);

    return {pM(1,1)/lDet,   -pM(0,1)/lDet,  (pM(0,1)*pM(1,2)-pM(1,1)*pM(0,2))/lDet,
            -pM(1,0)/lDet,  pM(0,0)/lDet,   (pM(1,0)*pM(0,2)-pM(0,0)*pM(1,2))/lDet};
}

inline mat::CMat23f inverse(const CMat23f &pM)
{
    float lDet=determinant(pM);

    return {pM(1,1)/lDet,   -pM(0,1)/lDet,  (pM(0,1)*pM(1,2)-pM(1,1)*pM(0,2))/lDet,
            -pM(1,0)/lDet,  pM(0,0)/lDet,   (pM(1,0)*pM(0,2)-pM(0,0)*pM(1,2))/lDet};
}

inline CPoint_d operator*(const CMat23d &pM,const CPoint_d &pP)
{
    return CPoint_d(pM(0,0)*pP.x+pM(0,1)*pP.y+pM(0,2),pM(1,0)*pP.x+pM(1,1)*pP.y+pM(1,2));
}

inline CPoint_d transform_vector(const CMat23d &pM,const CPoint_d &pP)
{
    return CPoint_d(pM(0,0)*pP.x+pM(0,1)*pP.y,pM(1,0)*pP.x+pM(1,1)*pP.y);
}

inline CSize_d transform_vector(const CMat23d &pM,const CSize_d &pP)
{
    return CSize_d(pM(0,0)*pP.x+pM(0,1)*pP.y,pM(1,0)*pP.x+pM(1,1)*pP.y);
}

inline CSlope_d transform_vector(const CMat23d &pM,const CSlope_d &pP)
{
    return CSlope_d(pM(0,0)*pP.x+pM(0,1)*pP.y,pM(1,0)*pP.x+pM(1,1)*pP.y);
}

inline CMat34d identity_3d()
{
    return {1.0,0.0,0.0,0.0,
            0.0,1.0,0.0,0.0,
            0.0,0.0,1.0,0.0};
}

inline CMat34f identity_3f()
{
    return {1.0,0.0,0.0,0.0,
            0.0,1.0,0.0,0.0,
            0.0,0.0,1.0,0.0};
}

inline CMat34d translation_3d(const CVec3d &pV)
{
    return {1.0,0.0,0.0,pV.x(),
            0.0,1.0,0.0,pV.y(),
            0.0,0.0,1.0,pV.z()};
}

inline CMat34f translation_3f(const CVec3f &pV)
{
    return {1.0,0.0,0.0,pV.x(),
            0.0,1.0,0.0,pV.y(),
            0.0,0.0,1.0,pV.z()};
}

inline CMat34d scaling_3d(double pS)
{
    return {pS ,0.0,0.0,0.0,
            0.0,pS ,0.0,0.0,
            0.0,0.0,pS ,0.0};
}

inline CMat34d scaling_3d(const CVec3d &pS)
{
    return {pS.x(),0.0   ,0.0   ,0.0,
            0.0   ,pS.y(),0.0   ,0.0,
            0.0   ,0.0   ,pS.z(),0.0};
}

inline CMat34d rotation_3d(const CQuat_d &pQ)
{
    return {1-2*pQ.y*pQ.y-2*pQ.z*pQ.z,2*pQ.x*pQ.y-2*pQ.w*pQ.z  ,2*pQ.x*pQ.z+2*pQ.w*pQ.y  ,0.0,
            2*pQ.x*pQ.y+2*pQ.w*pQ.z  ,1-2*pQ.x*pQ.x-2*pQ.z*pQ.z,2*pQ.y*pQ.z-2*pQ.w*pQ.x  ,0.0,
            2*pQ.x*pQ.z-2*pQ.w*pQ.y  ,2*pQ.y*pQ.z+2*pQ.w*pQ.x  ,1-2*pQ.x*pQ.x-2*pQ.y*pQ.y,0.0};
}

inline CMat34d rotation_translation_3d(const CQuat_d &pQ,const CVec3d &pT)
{
    return {1-2*pQ.y*pQ.y-2*pQ.z*pQ.z,2*pQ.x*pQ.y-2*pQ.w*pQ.z  ,2*pQ.x*pQ.z+2*pQ.w*pQ.y  ,pT.x(),
            2*pQ.x*pQ.y+2*pQ.w*pQ.z  ,1-2*pQ.x*pQ.x-2*pQ.z*pQ.z,2*pQ.y*pQ.z-2*pQ.w*pQ.x  ,pT.y(),
            2*pQ.x*pQ.z-2*pQ.w*pQ.y  ,2*pQ.y*pQ.z+2*pQ.w*pQ.x  ,1-2*pQ.x*pQ.x-2*pQ.y*pQ.y,pT.z()};
}

inline CMat34f rotation_translation_3f(const CQuat_f &pQ,const CVec3f &pT)
{
    return {1-2*pQ.y*pQ.y-2*pQ.z*pQ.z,2*pQ.x*pQ.y-2*pQ.w*pQ.z  ,2*pQ.x*pQ.z+2*pQ.w*pQ.y  ,pT.x(),
            2*pQ.x*pQ.y+2*pQ.w*pQ.z  ,1-2*pQ.x*pQ.x-2*pQ.z*pQ.z,2*pQ.y*pQ.z-2*pQ.w*pQ.x  ,pT.y(),
            2*pQ.x*pQ.z-2*pQ.w*pQ.y  ,2*pQ.y*pQ.z+2*pQ.w*pQ.x  ,1-2*pQ.x*pQ.x-2*pQ.y*pQ.y,pT.z()};
}

inline CMat34d rotation_x_3d(double pA)
{
    return {1.0     ,0.0     ,0.0     ,0.0,
            0.0     ,cos(pA) ,-sin(pA),0.0,
            0.0     ,sin(pA) ,cos(pA) ,0.0};
}

inline CMat34d rotation_y_3d(double pA)
{
    return {cos(pA) ,0.0     ,sin(pA) ,0.0,
            0.0     ,1.0     ,0.0     ,0.0,
            -sin(pA),0.0     ,cos(pA) ,0.0};
}

inline CMat34d rotation_z_3d(double pA)
{
    return {cos(pA) ,-sin(pA),0.0     ,0.0,
            sin(pA) ,cos(pA) ,0.0     ,0.0,
            0.0     ,0.0     ,1.0     ,0.0};
}

inline CMat34d transform_3d(const CVec3d &pO,const CVec3d &pX,const CVec3d &pY,const CVec3d &pZ)
{
    return {pX.x(),pY.x(),pZ.x(),pO.x(),
            pX.y(),pY.y(),pZ.y(),pO.y(),
            pX.z(),pY.z(),pZ.z(),pO.z()};
}

inline CMat34d cross_matrix(const CVec3d &pV)
{
    return {0.0    ,-pV.z(),pV.y() ,0.0,
            pV.z() ,0.0    ,-pV.x(),0.0,
            -pV.y(),pV.x() ,0.0    ,0.0};
}

inline CVec3d free_vector_product(const CMat34d &pM,const CVec3d &pV)
{
    return CVec3d({pM(0,0)*pV.x()+pM(0,1)*pV.y()+pM(0,2)*pV.z(),
                        pM(1,0)*pV.x()+pM(1,1)*pV.y()+pM(1,2)*pV.z(),
                        pM(2,0)*pV.x()+pM(2,1)*pV.y()+pM(2,2)*pV.z()});
}

inline CMat34d operator*(const CMat34d &pLH,const CMat34d &pRH)
{
    return {pLH(0,0)*pRH(0,0)+pLH(0,1)*pRH(1,0)+pLH(0,2)*pRH(2,0),
                        pLH(0,0)*pRH(0,1)+pLH(0,1)*pRH(1,1)+pLH(0,2)*pRH(2,1),
                                    pLH(0,0)*pRH(0,2)+pLH(0,1)*pRH(1,2)+pLH(0,2)*pRH(2,2),
                                                pLH(0,0)*pRH(0,3)+pLH(0,1)*pRH(1,3)+pLH(0,2)*pRH(2,3)+pLH(0,3),
            pLH(1,0)*pRH(0,0)+pLH(1,1)*pRH(1,0)+pLH(1,2)*pRH(2,0),
                        pLH(1,0)*pRH(0,1)+pLH(1,1)*pRH(1,1)+pLH(1,2)*pRH(2,1),
                                    pLH(1,0)*pRH(0,2)+pLH(1,1)*pRH(1,2)+pLH(1,2)*pRH(2,2),
                                                pLH(1,0)*pRH(0,3)+pLH(1,1)*pRH(1,3)+pLH(1,2)*pRH(2,3)+pLH(1,3),
            pLH(2,0)*pRH(0,0)+pLH(2,1)*pRH(1,0)+pLH(2,2)*pRH(2,0),
                        pLH(2,0)*pRH(0,1)+pLH(2,1)*pRH(1,1)+pLH(2,2)*pRH(2,1),
                                    pLH(2,0)*pRH(0,2)+pLH(2,1)*pRH(1,2)+pLH(2,2)*pRH(2,2),
                                                pLH(2,0)*pRH(0,3)+pLH(2,1)*pRH(1,3)+pLH(2,2)*pRH(2,3)+pLH(2,3)};
}

inline CMat4d operator*(const CMat4d &pLH,const CMat34d &pRH)
{
    return {pLH(0,0)*pRH(0,0)+pLH(0,1)*pRH(1,0)+pLH(0,2)*pRH(2,0),
                        pLH(0,0)*pRH(0,1)+pLH(0,1)*pRH(1,1)+pLH(0,2)*pRH(2,1),
                                    pLH(0,0)*pRH(0,2)+pLH(0,1)*pRH(1,2)+pLH(0,2)*pRH(2,2),
                                                pLH(0,0)*pRH(0,3)+pLH(0,1)*pRH(1,3)+pLH(0,2)*pRH(2,3)+pLH(0,3),
            pLH(1,0)*pRH(0,0)+pLH(1,1)*pRH(1,0)+pLH(1,2)*pRH(2,0),
                        pLH(1,0)*pRH(0,1)+pLH(1,1)*pRH(1,1)+pLH(1,2)*pRH(2,1),
                                    pLH(1,0)*pRH(0,2)+pLH(1,1)*pRH(1,2)+pLH(1,2)*pRH(2,2),
                                                pLH(1,0)*pRH(0,3)+pLH(1,1)*pRH(1,3)+pLH(1,2)*pRH(2,3)+pLH(1,3),
            pLH(2,0)*pRH(0,0)+pLH(2,1)*pRH(1,0)+pLH(2,2)*pRH(2,0),
                        pLH(2,0)*pRH(0,1)+pLH(2,1)*pRH(1,1)+pLH(2,2)*pRH(2,1),
                                    pLH(2,0)*pRH(0,2)+pLH(2,1)*pRH(1,2)+pLH(2,2)*pRH(2,2),
                                                pLH(2,0)*pRH(0,3)+pLH(2,1)*pRH(1,3)+pLH(2,2)*pRH(2,3)+pLH(2,3),
            pLH(3,0)*pRH(0,0)+pLH(3,1)*pRH(1,0)+pLH(3,2)*pRH(2,0),
                        pLH(3,0)*pRH(0,1)+pLH(3,1)*pRH(1,1)+pLH(3,2)*pRH(2,1),
                                    pLH(3,0)*pRH(0,2)+pLH(3,1)*pRH(1,2)+pLH(3,2)*pRH(2,2),
                                                pLH(3,0)*pRH(0,3)+pLH(3,1)*pRH(1,3)+pLH(3,2)*pRH(2,3)+pLH(3,3)};
}

inline CVec3d operator*(const CMat34d &pM,const CVec3d &pP)
{
    return {pM(0,0)*pP.x()+pM(0,1)*pP.y()+pM(0,2)*pP.z()+pM(0,3),
            pM(1,0)*pP.x()+pM(1,1)*pP.y()+pM(1,2)*pP.z()+pM(1,3),
            pM(2,0)*pP.x()+pM(2,1)*pP.y()+pM(2,2)*pP.z()+pM(2,3)};
}

inline CVec3f operator*(const CMat34f &pM,const CVec3f &pP)
{
    return {pM(0,0)*pP.x()+pM(0,1)*pP.y()+pM(0,2)*pP.z()+pM(0,3),
            pM(1,0)*pP.x()+pM(1,1)*pP.y()+pM(1,2)*pP.z()+pM(1,3),
            pM(2,0)*pP.x()+pM(2,1)*pP.y()+pM(2,2)*pP.z()+pM(2,3)};
}

inline CMat34d rotation_3d_around(const CQuat_d &pQ,const mat::CVec3d &pC)
{
    return translation_3d(pC)*rotation_3d(pQ)*translation_3d(-pC);
}

inline mat::CMat4d inverse(const mat::CMat4d &pM)
{
    double a0 = pM(0,0)*pM(1,1) - pM(0,1)*pM(1,0);
    double a1 = pM(0,0)*pM(1,2) - pM(0,2)*pM(1,0);
    double a2 = pM(0,0)*pM(1,3) - pM(0,3)*pM(1,0);
    double a3 = pM(0,1)*pM(1,2) - pM(0,2)*pM(1,1);
    double a4 = pM(0,1)*pM(1,3) - pM(0,3)*pM(1,1);
    double a5 = pM(0,2)*pM(1,3) - pM(0,3)*pM(1,2);
    double b0 = pM(2,0)*pM(3,1) - pM(2,1)*pM(3,0);
    double b1 = pM(2,0)*pM(3,2) - pM(2,2)*pM(3,0);
    double b2 = pM(2,0)*pM(3,3) - pM(2,3)*pM(3,0);
    double b3 = pM(2,1)*pM(3,2) - pM(2,2)*pM(3,1);
    double b4 = pM(2,1)*pM(3,3) - pM(2,3)*pM(3,1);
    double b5 = pM(2,2)*pM(3,3) - pM(2,3)*pM(3,2);

    double lDet = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
    if(std::abs(lDet)>std::numeric_limits<double>::epsilon())
    {
        mat::CMat4d lInverse;
        lInverse(0,0)= + pM(1,1)*b5 - pM(1,2)*b4 + pM(1,3)*b3;
        lInverse(1,0)= - pM(1,0)*b5 + pM(1,2)*b2 - pM(1,3)*b1;
        lInverse(2,0)= + pM(1,0)*b4 - pM(1,1)*b2 + pM(1,3)*b0;
        lInverse(3,0)= - pM(1,0)*b3 + pM(1,1)*b1 - pM(1,2)*b0;
        lInverse(0,1)= - pM(0,1)*b5 + pM(0,2)*b4 - pM(0,3)*b3;
        lInverse(1,1)= + pM(0,0)*b5 - pM(0,2)*b2 + pM(0,3)*b1;
        lInverse(2,1)= - pM(0,0)*b4 + pM(0,1)*b2 - pM(0,3)*b0;
        lInverse(3,1)= + pM(0,0)*b3 - pM(0,1)*b1 + pM(0,2)*b0;
        lInverse(0,2)= + pM(3,1)*a5 - pM(3,2)*a4 + pM(3,3)*a3;
        lInverse(1,2)= - pM(3,0)*a5 + pM(3,2)*a2 - pM(3,3)*a1;
        lInverse(2,2)= + pM(3,0)*a4 - pM(3,1)*a2 + pM(3,3)*a0;
        lInverse(3,2)= - pM(3,0)*a3 + pM(3,1)*a1 - pM(3,2)*a0;
        lInverse(0,3)= - pM(2,1)*a5 + pM(2,2)*a4 - pM(2,3)*a3;
        lInverse(1,3)= + pM(2,0)*a5 - pM(2,2)*a2 + pM(2,3)*a1;
        lInverse(2,3)= - pM(2,0)*a4 + pM(2,1)*a2 - pM(2,3)*a0;
        lInverse(3,3)= + pM(2,0)*a3 - pM(2,1)*a1 + pM(2,2)*a0;

        double lInvDet = ((double)1)/lDet;
        lInverse(0,0) *= lInvDet;
        lInverse(0,1) *= lInvDet;
        lInverse(0,2) *= lInvDet;
        lInverse(0,3) *= lInvDet;
        lInverse(1,0) *= lInvDet;
        lInverse(1,1) *= lInvDet;
        lInverse(1,2) *= lInvDet;
        lInverse(1,3) *= lInvDet;
        lInverse(2,0) *= lInvDet;
        lInverse(2,1) *= lInvDet;
        lInverse(2,2) *= lInvDet;
        lInverse(2,3) *= lInvDet;
        lInverse(3,0) *= lInvDet;
        lInverse(3,1) *= lInvDet;
        lInverse(3,2) *= lInvDet;
        lInverse(3,3) *= lInvDet;

        return lInverse;
    }

    return {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
}

inline mat::CMat34d inverse(const mat::CMat34d &pM)
{
    double a0 = pM(0,0)*pM(1,1) - pM(0,1)*pM(1,0);
    double a1 = pM(0,0)*pM(1,2) - pM(0,2)*pM(1,0);
    double a2 = pM(0,0)*pM(1,3) - pM(0,3)*pM(1,0);
    double a3 = pM(0,1)*pM(1,2) - pM(0,2)*pM(1,1);
    double a4 = pM(0,1)*pM(1,3) - pM(0,3)*pM(1,1);
    double a5 = pM(0,2)*pM(1,3) - pM(0,3)*pM(1,2);
    double b2 = pM(2,0);
    double b4 = pM(2,1);
    double b5 = pM(2,2);

    double lDet = a0*b5 - a1*b4 + a3*b2;
    if(std::abs(lDet)>std::numeric_limits<double>::epsilon())
    {
        mat::CMat34d lInverse;
        lInverse(0,0)= + pM(1,1)*b5 - pM(1,2)*b4;
        lInverse(1,0)= - pM(1,0)*b5 + pM(1,2)*b2;
        lInverse(2,0)= + pM(1,0)*b4 - pM(1,1)*b2;
        lInverse(0,1)= - pM(0,1)*b5 + pM(0,2)*b4;
        lInverse(1,1)= + pM(0,0)*b5 - pM(0,2)*b2;
        lInverse(2,1)= - pM(0,0)*b4 + pM(0,1)*b2;
        lInverse(0,2)= a3;
        lInverse(1,2)= -a1;
        lInverse(2,2)= a0;
        lInverse(0,3)= - pM(2,1)*a5 + pM(2,2)*a4 - pM(2,3)*a3;
        lInverse(1,3)= + pM(2,0)*a5 - pM(2,2)*a2 + pM(2,3)*a1;
        lInverse(2,3)= - pM(2,0)*a4 + pM(2,1)*a2 - pM(2,3)*a0;

        double lInvDet = 1.0/lDet;
        lInverse(0,0) *= lInvDet;
        lInverse(0,1) *= lInvDet;
        lInverse(0,2) *= lInvDet;
        lInverse(0,3) *= lInvDet;
        lInverse(1,0) *= lInvDet;
        lInverse(1,1) *= lInvDet;
        lInverse(1,2) *= lInvDet;
        lInverse(1,3) *= lInvDet;
        lInverse(2,0) *= lInvDet;
        lInverse(2,1) *= lInvDet;
        lInverse(2,2) *= lInvDet;
        lInverse(2,3) *= lInvDet;

        return lInverse;
    }

    return {0,0,0,0,0,0,0,0,0,0,0,0};
}

inline mat::CMat34f inverse(const mat::CMat34f &pM)
{
    float a0 = pM(0,0)*pM(1,1) - pM(0,1)*pM(1,0);
    float a1 = pM(0,0)*pM(1,2) - pM(0,2)*pM(1,0);
    float a2 = pM(0,0)*pM(1,3) - pM(0,3)*pM(1,0);
    float a3 = pM(0,1)*pM(1,2) - pM(0,2)*pM(1,1);
    float a4 = pM(0,1)*pM(1,3) - pM(0,3)*pM(1,1);
    float a5 = pM(0,2)*pM(1,3) - pM(0,3)*pM(1,2);
    float b2 = pM(2,0);
    float b4 = pM(2,1);
    float b5 = pM(2,2);

    float lDet = a0*b5 - a1*b4 + a3*b2;
    if(std::abs(lDet)>std::numeric_limits<float>::epsilon())
    {
        mat::CMat34f lInverse;
        lInverse(0,0)= + pM(1,1)*b5 - pM(1,2)*b4;
        lInverse(1,0)= - pM(1,0)*b5 + pM(1,2)*b2;
        lInverse(2,0)= + pM(1,0)*b4 - pM(1,1)*b2;
        lInverse(0,1)= - pM(0,1)*b5 + pM(0,2)*b4;
        lInverse(1,1)= + pM(0,0)*b5 - pM(0,2)*b2;
        lInverse(2,1)= - pM(0,0)*b4 + pM(0,1)*b2;
        lInverse(0,2)= a3;
        lInverse(1,2)= -a1;
        lInverse(2,2)= a0;
        lInverse(0,3)= - pM(2,1)*a5 + pM(2,2)*a4 - pM(2,3)*a3;
        lInverse(1,3)= + pM(2,0)*a5 - pM(2,2)*a2 + pM(2,3)*a1;
        lInverse(2,3)= - pM(2,0)*a4 + pM(2,1)*a2 - pM(2,3)*a0;

        float lInvDet = 1.0f/lDet;
        lInverse(0,0) *= lInvDet;
        lInverse(0,1) *= lInvDet;
        lInverse(0,2) *= lInvDet;
        lInverse(0,3) *= lInvDet;
        lInverse(1,0) *= lInvDet;
        lInverse(1,1) *= lInvDet;
        lInverse(1,2) *= lInvDet;
        lInverse(1,3) *= lInvDet;
        lInverse(2,0) *= lInvDet;
        lInverse(2,1) *= lInvDet;
        lInverse(2,2) *= lInvDet;
        lInverse(2,3) *= lInvDet;

        return lInverse;
    }

    return {0,0,0,0,0,0,0,0,0,0,0,0};
}

inline double determinant(const mat::CMat4d &pM)
{
    double a0 = pM(0,0)*pM(1,1) - pM(0,1)*pM(1,0);
    double a1 = pM(0,0)*pM(1,2) - pM(0,2)*pM(1,0);
    double a2 = pM(0,0)*pM(1,3) - pM(0,3)*pM(1,0);
    double a3 = pM(0,1)*pM(1,2) - pM(0,2)*pM(1,1);
    double a4 = pM(0,1)*pM(1,3) - pM(0,3)*pM(1,1);
    double a5 = pM(0,2)*pM(1,3) - pM(0,3)*pM(1,2);
    double b0 = pM(2,0)*pM(3,1) - pM(2,1)*pM(3,0);
    double b1 = pM(2,0)*pM(3,2) - pM(2,2)*pM(3,0);
    double b2 = pM(2,0)*pM(3,3) - pM(2,3)*pM(3,0);
    double b3 = pM(2,1)*pM(3,2) - pM(2,2)*pM(3,1);
    double b4 = pM(2,1)*pM(3,3) - pM(2,3)*pM(3,1);
    double b5 = pM(2,2)*pM(3,3) - pM(2,3)*pM(3,2);

    return a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
}

inline CMat34d look_at(const mat::CVec3d &pAt,const mat::CVec3d &pEye,const mat::CVec3d &pUp)
{
    mat::CVec3d lFront=pAt-pEye;
    lFront/=norm(lFront);
    mat::CVec3d lRight=cross(lFront,pUp);
    lRight/=norm(lRight);
    mat::CVec3d lUp=cross(lRight,lFront);

    return {lRight.x(),lRight.y(),lRight.z(),-dot(lRight,pEye),
            lUp.x(),lUp.y(),lUp.z(),-dot(lUp,pEye),
            -lFront.x(),-lFront.y(),-lFront.z(),dot(lFront,pEye)};
}

inline CMat4d perspective(double pFOVy,double pAspect,double pNear,double pFar)
{
    double lF=1.0/std::tan(pFOVy/2);

    return {lF/pAspect,0.0,0.0                      ,0.0,
            0.0       ,lF ,0.0                      ,0.0,
            0.0       ,0.0,(pFar+pNear)/(pNear-pFar),2*pFar*pNear/(pNear-pFar),
            0.0       ,0.0,-1.0                     ,0.0};
}

inline void shift_perspective_center(CMat4d &pM,const CPoint_d &pShift,const CSize_d &pSize=CSize_d(1,1))
{
    pM(0,2)=-2*pShift.x/pSize.x;
    pM(1,2)=2*pShift.y/pSize.y;
}

//! angle between two vectors, using pRef as the normal of the plane where the rotation is CCW
inline double angle(const CVec3d &p1,const CVec3d &p2,const CVec3d &pRef)
{
    CVec3d l1=p1/norm(p1);
    CVec3d l2=p2/norm(p2);
    double lDot=dot(l1,l2);
    CVec3d lCross=cross(l1,l2);
    double lAngle=acos(lDot);
    if(dot(lCross,pRef)<0.0)
        return -lAngle;
    else
        return lAngle;
}

/*namespace mat*/ } /*namespace buola*/ }

#endif
